#include "encoder.h"
#include "main.h"
#include "i2c.h"
#include "oled.h"
//#include "BLDC.h"

#define _2PI 6.28318

static int rotation = 0;
int direction = 1;
float angle_data_prev = 0;
static float last_angle = 0;

float total_angle = 0;
float raw_angle=0;
float vel = 0;

void AS5600_update()
{
   total_angle = full_angle();
   vel = get_velocity(total_angle);
}
float AS5600_get_raw()
{
   return raw_angle;
}
float AS5600_get_full()
{
   return total_angle;
}
float AS5600_get_vel()
{
   return vel;
}


void as5600Init(void) {
  rotation = 0;
  angle_data_prev = as5600GetRawAngle();
}

uint32_t low_pass_fliter(uint32_t angle){
	static uint32_t full;
	full = full*0.9 + angle *0.1;
	return full;
}

float as5600GetRawAngle(void) //耗时700us，0.7ms，理论频率可达1k
{
   uint8_t buffer[2] = {0};
   uint8_t addr = 0x0c;
   // HAL_I2C_Master_Transmit_DMA(&hi2c1, (0x36<<1), &addr, 1);
   // HAL_I2C_Master_Receive_DMA(&hi2c1, (0x36<<1)|1, buffer, 2);
   HAL_I2C_Master_Transmit(&hi2c1, (0x36<<1), &addr, 1, 10);
   HAL_I2C_Master_Receive(&hi2c1, (0x36<<1)|1, buffer, 2, 10);
   uint32_t angle_data = buffer[0] << 8 | buffer[1];
   float return_value = (angle_data / (float)AS5600_RESOLUTION)*_2PI;
   return return_value;
}

void full_rotation(int num){
   static int last = 0;
   if(num-last == 6){
      rotation --;
   }else if(num - last == -6){
      rotation ++;
      
   }
   last = num;
}

float full_angle(void){
	float angle = 0;
   angle = as5600GetRawAngle();
   raw_angle = angle;
   full_rotation((int)angle);
   //printf("%d,%.2lf,angle = %.2lf\n",rotation,(float)rotation*_2PI + angle,angle);
	return ((float)rotation*_2PI + angle - angle_data_prev)*(float)direction;
}

float get_velocity(float angle){
   float Ts = 0.005;
   float vel = (angle - last_angle)/Ts;
   last_angle = angle;
   return vel;
}




